#!/usr/bin/env python
# -*- coding: UTF-8 -*-

import RPi.GPIO as GPIO
import time
import atexit


class Controller:
    def __init__(self):
        atexit.register(GPIO.cleanup)
        self.pitch_pin = 17  # 俯仰角控制
        self.yaw_pin = 27  # 航向角控制

        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.pitch_pin, GPIO.OUT, initial=False)
        self.pitch_pwm = GPIO.PWM(self.pitch_pin, 50)
        self.pitch_pwm.start(0)
        GPIO.setup(self.yaw_pin, GPIO.OUT, initial=False)
        self.yaw_pwm = GPIO.PWM(self.yaw_pin, 50)
        self.yaw_pwm.start(0)
        self.reset()
        time.sleep(2)

    def control_pitch(self, pitch_angle):
        if pitch_angle>120:
            pitch_angle=120
        elif pitch_angle<1:
            pitch_angle=1
        dutyCycle = 2.5 + 10.* pitch_angle / 180.
        print dutyCycle
        self.pitch_pwm.ChangeDutyCycle(dutyCycle)
        time.sleep(0.02)

    def control_yaw(self, yaw_angle):
        if yaw_angle>165:
            yaw_angle=165
        elif yaw_angle<15:
            yaw_angle=15
        dutyCycle = 2.5 + 10. * yaw_angle / 180.
        print dutyCycle
        self.yaw_pwm.ChangeDutyCycle(dutyCycle)
        time.sleep(0.02)

    def control_pitch_new(self, dutyCycle):
        # dutyCycle = dy*6.5+6.5
        if dutyCycle>9.1667:
            dutyCycle=9.1667
        elif dutyCycle<2.5:
            dutyCycle=2.5
        self.pitch_pwm.ChangeDutyCycle(dutyCycle)
        time.sleep(0.02)

    def control_yaw_new(self, dutyCycle):
        # dutyCycle = dx*7.5+7.5
        if dutyCycle>11.6667:
            dutyCycle=11.6667
        elif dutyCycle<3.3333:
            dutyCycle=3.3333
        self.yaw_pwm.ChangeDutyCycle(dutyCycle)
        time.sleep(0.02)

    def reset(self):
        self.pitch_pwm.ChangeDutyCycle(6.5)
        self.yaw_pwm.ChangeDutyCycle(7.5)
        time.sleep(0.02)

if __name__ == '__main__':
    mController_test = Controller()
    while True:
        ch = raw_input("请选择控制pitch(p)或yaw(y)或reset(r):")
        # if ch == 'p':
        #     pitch_angle_test = input("俯仰角（1-125）：")
        #     mController_test.control_pitch(pitch_angle_test)
        # elif ch == 'y':
        #     yaw_angle_test = input("航向角（30-160）：")
        #     mController_test.control_yaw(yaw_angle_test)
        # elif ch=='r':
        #     mController_test.reset()

        if ch == 'p':
            pitch_angle_test = input("俯仰角（2.5-9.1）：")
            mController_test.control_pitch_new(pitch_angle_test)
        elif ch == 'y':
            yaw_angle_test = input("航向角（3.3-11.6）：")
            mController_test.control_yaw_new(yaw_angle_test)
        elif ch=='r':
            mController_test.reset()